#include "bspline_opt/uniform_bspline.h"
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Twist.h"
#include "ego_planner/Bspline.h"
#include "std_msgs/UInt8.h"
#include "std_msgs/UInt8MultiArray.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf/transform_listener.h"
#include "tf/transform_datatypes.h"
//#include "quadrotor_msgs/PositionCommand.h"
#include "std_msgs/Empty.h"
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include "ego_planner/PoseAdjust.h"
#include "std_msgs/UInt8.h"
#include "visualization_msgs/Marker.h"
#include <ros/ros.h>
#include "time.h"

#define PI 3.1415926
#define yaw_error_max 90.0/180*PI
#define N 15

ros::Publisher vel_cmd_pub;
ros::Publisher replan_pub;
ros::Publisher diresed_pose_pub;

//quadrotor_msgs::PositionCommand cmd;
geometry_msgs::Twist cmd;
double pos_gain[3] = {0, 0, 0};
double vel_gain[3] = {0, 0, 0};

using ego_planner::UniformBspline;

bool receive_traj_ = false;
bool is_orientation_init = false;
bool need_to_adjust_pose_ = false;
double target_yaw_;
vector<UniformBspline> traj_;
double traj_duration_;
ros::Time start_time_,time_s,time_e;
int traj_id_;

Eigen::Vector3d odom_pos_,odom_vel_;
Eigen::Quaterniond odom_orient_;

double roll,pitch,yaw;
geometry_msgs::PoseStamped pose_cur;
tf::Quaternion quat;
std_msgs::UInt8 is_adjust_pose;
std_msgs::UInt8 dir;

enum DIRECTION {POSITIVE=0,NEGATIVE=1};

// yaw control
double t_step;

std_msgs::UInt8 stop_command;

////time record
clock_t start_clock,end_clock;
double duration;

void bsplineCallback(ego_planner::BsplineConstPtr msg)
{
  // parse pos traj

  Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());

  Eigen::VectorXd knots(msg->knots.size());
  for (size_t i = 0; i < msg->knots.size(); ++i)
  {
    knots(i) = msg->knots[i];
  }

  for (size_t i = 0; i < msg->pos_pts.size(); ++i)
  {
    pos_pts(0, i) = msg->pos_pts[i].x;
    pos_pts(1, i) = msg->pos_pts[i].y;
    pos_pts(2, i) = msg->pos_pts[i].z;
  }

  UniformBspline pos_traj(pos_pts, msg->order, 0.1);
  pos_traj.setKnot(knots);


  start_time_ = msg->start_time;
  traj_id_ = msg->traj_id;

  traj_.clear();
  traj_.push_back(pos_traj);
  traj_.push_back(traj_[0].getDerivative());
  traj_.push_back(traj_[1].getDerivative());

  traj_duration_ = traj_[0].getTimeSum();

  ROS_WARN("Receive b-spline trajectory!");

  receive_traj_ = true;

}

void poseCallback(geometry_msgs::PoseStampedConstPtr msg)
{
    pose_cur = *msg;
    tf::quaternionMsgToTF(msg->pose.orientation, quat);
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
}

void adjust_yaw_Callback(std_msgs::UInt8ConstPtr msg)
{
    is_adjust_pose = *msg;
}

void dirCallback(const std_msgs::UInt8ConstPtr& msg)
{
    dir = *msg;
}

struct PoseDeviation {
  double lateral{0.0};
  double longitudinal{0.0};
  double yaw{0.0};
};

inline double normalizeRadian(const double rad, const double min_rad = -PI) {
  const auto max_rad = min_rad + 2 * PI;

  const auto value = std::fmod(rad, 2 * PI);
  if (min_rad <= value && value < max_rad) {
    return value;
  }

  return value - std::copysign(2 * PI, value);
}

inline double calcYawDeviation(
  const double & base_yaw, const double & target_yaw) {
  return normalizeRadian(target_yaw - base_yaw);
}

inline double calcLateralDeviation(
  const Eigen::Vector3d & base_point, const double & base_yaw, const Eigen::Vector3d & target_point) {
  const Eigen::Vector3d base_unit_vec{std::cos(base_yaw), std::sin(base_yaw), 0};

  const Eigen::Vector3d diff_vec = target_point - base_point;

  const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec);

  return cross_vec.z();
}

void PP_controller(double &t_cur) {
  Eigen::Vector2d car_pose(odom_pos_(0), odom_pos_(1));
  double match_t = 0.0;
  double min_distance = 9999;
  for (double cur_t=0.0;cur_t <= traj_duration_; cur_t += 0.1) {
    Eigen::Vector2d cur_point(traj_[0].evaluateDeBoor(cur_t)(0), traj_[0].evaluateDeBoor(cur_t)(1));
    double distance = (cur_point - car_pose).norm();
    if(distance < min_distance) {
      min_distance = distance;
      match_t = cur_t;
    }
  }
  double best_match_t = match_t;
  const double ld = 1.5;
  // 判断是否接近终点了，若接近，直接选最后一个点为前视点
  if ((car_pose - Eigen::Vector2d(traj_[0].evaluateDeBoor(traj_duration_)(0), traj_[0].evaluateDeBoor(traj_duration_)(1))).norm() < ld) {
    match_t = traj_duration_-0.1;
  } else {
    for (double cur_t = match_t; cur_t <= traj_duration_; cur_t += 0.1) {
      Eigen::Vector2d cur_point(traj_[0].evaluateDeBoor(cur_t)(0), traj_[0].evaluateDeBoor(cur_t)(1));
      double distance = (cur_point - car_pose).norm();
      if (distance > ld) {
        match_t = cur_t;
        break;
      }
    }
  }
  t_cur = match_t;
  Eigen::Vector3d desired_point = traj_[0].evaluateDeBoor(t_cur);
  Eigen::Vector3d desired_vel = traj_[1].evaluateDeBoor(t_cur);
  // 考虑末端速度不稳定，使用位置做朝向解算
  if (t_cur > 0.5) {
    Eigen::Vector3d before_desired_point = traj_[0].evaluateDeBoor(t_cur-0.5);
    desired_vel = (desired_point - before_desired_point);
  }
  double base_yaw = atan2(desired_vel(1),desired_vel(0));
  
  geometry_msgs::PoseStamped vis_msg;
  vis_msg.pose.position.x = desired_point(0);
  vis_msg.pose.position.y = desired_point(1);
  Eigen::AngleAxisd yaw_rotation(base_yaw, Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond q(yaw_rotation);
  vis_msg.header.stamp = ros::Time::now();
  vis_msg.header.frame_id = "map";
  vis_msg.pose.orientation.w = q.w();
  vis_msg.pose.orientation.x = q.x();
  vis_msg.pose.orientation.y = q.y();
  vis_msg.pose.orientation.z = q.z();
  diresed_pose_pub.publish(vis_msg);


  double lat_err = calcLateralDeviation(desired_point, base_yaw, odom_pos_);
  double yaw_err = calcYawDeviation(base_yaw, yaw);

  double _em_contorl_ld = 1.5;
  if (std::abs(lat_err) > _em_contorl_ld) {
    _em_contorl_ld = std::abs(lat_err)+0.5;
  }
  double alpha = std::asin(lat_err / _em_contorl_ld) + yaw_err;
  double lv = 0.5;
  double av = 0.0;
  if (std::abs(alpha) < 0.017) {
    av = 0;
  } else {
    double R = -_em_contorl_ld / (2 * std::sin(alpha));
    av = lv / R;
  }

  cmd.linear.x = lv;
  cmd.angular.z = av;

  t_cur = best_match_t;
  // std::cout << "lat_err : " << lat_err << ", yaw_err : " << yaw_err << std::endl;
  // std::cout << "lv : " << lv << ", av : " << av << std::endl;
}

void PoseAdjust() {
  double desired_yaw = std::fmod(target_yaw_, 2 * M_PI);
  double angle_diff = std::atan2(std::sin(yaw - desired_yaw), std::cos(yaw - desired_yaw));

  // std::cout << "[Debug] cur yaw: " << yaw << ", desired_yaw: " << desired_yaw << "; angle diff: " << angle_diff << std::endl;
  const double max_w = 0.5;
  const double min_w = 0.2;
  cmd.linear.x = 0;
  cmd.angular.z = std::max(std::min(max_w * std::abs(angle_diff), max_w), min_w) * -std::copysign(1, angle_diff);
  return;
  // if (std::abs(angle_diff) > 0.174) { // 10 度
  //   cmd.linear.x = 0;
  //   cmd.angular.z = std::max(std::min(max_w * std::abs(angle_diff), max_w), min_w) * -std::copysign(1, angle_diff);
  //   return;
  // }
}

void stopCallback(std_msgs::UInt8ConstPtr msg)
{
    stop_command = *msg;
}

void odometryCallback(const nav_msgs::OdometryConstPtr &msg)
{
    odom_pos_(0) = msg->pose.pose.position.x;
    odom_pos_(1) = msg->pose.pose.position.y;
    odom_pos_(2) = msg->pose.pose.position.z;

    odom_vel_(0) = msg->twist.twist.linear.x;
    odom_vel_(1) = msg->twist.twist.linear.y;
    odom_vel_(2) = msg->twist.twist.linear.z;

    //odom_acc_ = estimateAcc( msg );

    odom_orient_.w() = msg->pose.pose.orientation.w;
    odom_orient_.x() = msg->pose.pose.orientation.x;
    odom_orient_.y() = msg->pose.pose.orientation.y;
    odom_orient_.z() = msg->pose.pose.orientation.z;

    tf::quaternionMsgToTF(msg->pose.pose.orientation,quat);
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    if(dir.data==NEGATIVE)
    {
        if(yaw>0)
        {
            yaw -= PI;
        }else if(yaw<0)
        {
            yaw += PI;
        }
    }

}

void cmdCallback(const ros::TimerEvent &e)
{
    /* no publishing before receive traj_ */
    if (stop_command.data==1)
    {
        cmd.angular.z = 0;
        cmd.linear.x = 0;
        vel_cmd_pub.publish(cmd);
        return;
    }

    if (need_to_adjust_pose_) {
      PoseAdjust();
      vel_cmd_pub.publish(cmd);
      return;
    }

    if (!receive_traj_) {
      cmd.angular.z = 0;
      cmd.linear.x = 0;
      vel_cmd_pub.publish(cmd);
      return;
    }
    //ROS_WARN("Run here !");
    ros::Time time_s = ros::Time::now();
    double t_cur = (time_s - start_time_).toSec();
    PP_controller(t_cur);

    if (std::abs(t_cur - traj_duration_) < 0.2) {
      cmd.angular.z = 0;
      cmd.linear.x = 0;
      vel_cmd_pub.publish(cmd);
      std_msgs::Bool replan_msg;
      replan_msg.data = true;
      replan_pub.publish(replan_msg);
      receive_traj_ = false;
      ROS_WARN("TRAJ EXE A TRAJ!");
      return;
    }
    vel_cmd_pub.publish(cmd);
//    Eigen::Vector3d pos_first = traj_[0].evaluateDeBoor(t_cur);
//    Eigen::Vector3d pos_second = traj_[0].evaluateDeBoor(t_cur+t_step);
//    double yaw_start = atan2((pos_second-pos_first)(1),(pos_second-pos_first)(0));

    // static ros::Time time_last = ros::Time::now();

    // if (t_cur < traj_duration_ && t_cur >= 0.0)
    // {
    //     //ROS_INFO("MPC_Calculate!");
    //     start_clock = clock();
    //     PP_controller(t_cur);
    //     end_clock = clock();
    //     duration = (double)(end_clock - start_clock) / CLOCKS_PER_SEC *1000;
    //     //ROS_INFO("Control times : %f ms",duration);
    // }
    // else if (t_cur >= traj_duration_)
    // {
    //     cmd.angular.z = 0;
    //     cmd.linear.x = 0;
    //     vel_cmd_pub.publish(cmd);
    //     is_orientation_init=false;
    // }
    // else
    // {
    //     cout << "[Traj server]: invalid time." << endl;
    // }
    // time_last = time_s;

    // vel_cmd_pub.publish(cmd);
}

void AdjustPoseCallback(const ego_planner::PoseAdjustConstPtr &msg) {
  need_to_adjust_pose_ = msg->need_to_adjust;
  target_yaw_ = msg->target_yaw;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "traj_server");
  ros::NodeHandle node("~");

  std::string cmd_topic,pose_topic;
  node.getParam("/ego_planner_node/fsm/pose_topic",pose_topic);
  node.getParam("/ego_planner_node/fsm/vel_topic",cmd_topic);


  ros::Subscriber bspline_sub = node.subscribe("/planning/bspline", 10, bsplineCallback);
  ros::Subscriber pose_sub = node.subscribe(pose_topic, 10, poseCallback);
  ros::Subscriber odom_sub = node.subscribe("/state_estimation", 10, odometryCallback);
  ros::Subscriber stop_sub = node.subscribe("/emergency_stop",10,stopCallback);
  ros::Subscriber adjust_yaw_sub = node.subscribe("/is_adjust_yaw",10,adjust_yaw_Callback);
  ros::Subscriber command_sub = node.subscribe("/direction",10,dirCallback);
  ros::Subscriber adjust_pose_sub = node.subscribe("/adjust_pose",10,AdjustPoseCallback);

  replan_pub = node.advertise<std_msgs::Bool>("/kk/need_replan", 10);
  diresed_pose_pub = node.advertise<geometry_msgs::PoseStamped>("/kk/desired_pose",10);

  vel_cmd_pub = node.advertise<geometry_msgs::Twist>("/inner_algorithm_cmd_vel", 50);
  stop_command.data = 0;
  dir.data = POSITIVE;
  t_step = 0.03;


  ros::Timer cmd_timer = node.createTimer(ros::Duration(0.03), cmdCallback);

//  nh.param("traj_server/time_forward", time_forward_, -1.0);
//  last_yaw_ = 0.0;
//  last_yaw_dot_ = 0.0;

  ros::Duration(1.0).sleep();

  ROS_WARN("[Traj server]: ready.");

  ros::spin();

  return 0;
}